#include <ros/ros.h>

#include <iostream>
#include <algorithm>
#include <memory>
#include <cmath>
#include <thread>
#include <future>
#include <atomic>
#include <boost/circular_buffer.hpp>
#include "class_custom.h"

#include <control_msgs/control_msg.h>
#include <actuator_msgs/actuator_msg.h>

#include "actuator/param_srv/param_srv.h"
#include "actuator/kinematic/kinematic_base.h"
#include "actuator/kinematic/kinematic_normal.h"
#include "actuator/vcs.h"

#include "common_lib/libtime.h"
#include "common_lib/log.h"
#include "signal.h"

namespace actuator
{
class ActuatorMain
{
public:
    ActuatorMain(ros::NodeHandle &nh, ros::NodeHandle &private_nh, std::string flie, std::string log_path);
    ~ActuatorMain();

    void Init(std::string file);
    void Start(void);

private:
    ros::NodeHandle nh_;
    ros::NodeHandle private_nh_;

    int loop_ = 100;
    static bool running;

    common_lib::Common com_;

    control_msgs::control_msg msg_control_;
    ros::Subscriber sub_control_;
    ros::Publisher pub_actuator_;

    actuator_msgs::actuator_msg msg_actuator_;

    std::shared_ptr<boost::circular_buffer<st_par>> read_buffer_;
    std::shared_ptr<boost::circular_buffer<st_par>> write_buffer_;
    AMRCmdAndState amr_state;
    // kinematic model
    std::shared_ptr<AMRKinematicModeBase> ptr_kinematic_;
    // vcs
    static std::shared_ptr<cls_lib> ptr_vcs_;
    // GetParams
    std::shared_ptr<GetParams> ptr_params_;

    std::vector<std::future<int>> v_futures;
    std::string param_file_;
    std::string log_path_;

    void InitLog();

    void ControlCallback(const control_msgs::control_msgConstPtr &msg);
    void GetState();
    void CalcSpeed();

    static void SigHandler(int sig);
};
}